#ifndef SRC_APPSW_TRICORE_USER_LQ_PID_H_
#define SRC_APPSW_TRICORE_USER_LQ_PID_H_

#include <stdint.h>
#include "zf_common_headfile.h"

extern uint8_t stop_flag;

extern volatile int16_t export; //输出
extern volatile int16_t Sout;
extern volatile int16_t SoutL;
extern volatile int16_t SoutR;
extern volatile int16_t MoutL;
extern volatile int16_t MoutR;

//extern int16_t Target_Speed;

typedef struct
{
  float                kp;         //P
  float                ki;         //I
  float                kd;         //D
  float                imax;       //积分限幅

  float                out_p;  //KP输出
  float                out_i;  //KI输出
  float                out_d;  //KD输出
  float                out;    //pid输出

  float                integrator; //< 积分值
  float                last_error; //< 上次误差
  float                last_derivative;//< 上次误差与上上次误差之差
  unsigned long        last_t;     //< 上次时间
}pid_param_t;

typedef struct
{
  float                kp;         //P
  float                kd;         //D

  float                out_p;  //KP输出
  float                out_d;  //KD输出
  float                out;    //pid输出

  float                last_error; //< 上次误差
}pd_param_t;

void PidInit(pid_param_t * pid);

float constrain_float(float amt, float low, float high);

float PidLocCtrl(pid_param_t * pid, float error);

float PidIncCtrl(pid_param_t * pid, float error);

float PidCasCtrl(pid_param_t *external_pid, pid_param_t *interior_pid, float error);

void Clipping(int16* spend,int16 min,int16 max);

void My_Pid_Init(void);
void Calculat_Out(void);
void buletooth_control(char buf);
void change_speed(int16_t error);
float PdLocCtrl(pd_param_t *pd_Speed,pd_param_t *pd_Cascade, float error);

#endif /* SRC_APPSW_TRICORE_USER_LQ_PID_H_ */

